#include <core/test/planner_reactive_test.h>
#include <iostream>

using std::cout;
using std::cin;
using std::endl;

using PlayerCc::dtor;

static const double kPi = 3.14159265;

int main(int argc, char* argv[]) {
  //player_point_2d initial_position, goal_position;
  //double initial_yaw, goal_yaw;

  // Set up player's objects.
  PlayerCc::PlayerClient player_client("localhost");
  PlayerCc::Position2dProxy position2d_proxy(&player_client);
  //PlayerCc::SonarProxy sonar_proxy(&player_client);
  //PathPlanner path_planner(argv[1], argv[2]);

  position2d_proxy.RequestGeom();
  //sonar_proxy.RequestGeom();
  player_client.Read();

  // Get mission.
  /*
  cout << "Initial pose (px py pa). pa in degrees.: ";
  cin >> initial_position.px >> initial_position.py >> initial_yaw;
  initial_yaw = dtor(initial_yaw);
  cout << "Goal pose (px py pa): ";
  cin >> goal_position.px >> goal_position.py >> goal_yaw;
  goal_yaw = dtor(goal_yaw);
  */
  /*
  initial_position.px = -23;
  initial_position.py = 2;
  initial_yaw = dtor(45);
  goal_position.px = 6;
  goal_position.py = 1;
  goal_yaw = 0;
  position2d_proxy.SetOdometry(initial_position.px, initial_position.py, initial_yaw);
  */
  position2d_proxy.SetMotorEnable(1);
  position2d_proxy.SetSpeed(0.3, 0);
  boost::this_thread::sleep(boost::posix_time::seconds(3)); 
  position2d_proxy.SetSpeed(0, 0);
  return 0;
}
